#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");

// 这里是发送位姿的函数，只要zbw把坐标发给我，我判断和之前是不是重复，如果不是那我就去
// zbw给pose topic发送位姿
class EegoalNode : public rclcpp::Node
{

public:
    EegoalNode(std::string name) : Node(name)
    {
        auto qos = rclcpp::QoS(1).reliable().transient_local();
        pub_goal = this->create_publisher<geometry_msgs::msg::Pose>(
            "ee_goal_pose",
            qos);
        sub_goal = this->create_subscription<geometry_msgs::msg::Pose>(
            "pose",
            10,
            std::bind(
                &EegoalNode::ee_pose_callback,
                this,
                std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "Published goal pose");
    };

private:
    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr pub_goal;
    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr sub_goal;
    geometry_msgs::msg::Pose pose;
    geometry_msgs::msg::Pose last_pose;
    rclcpp::TimerBase::SharedPtr timer_;
    void ee_pose_callback(const geometry_msgs::msg::Pose::SharedPtr msg)
    {
        // 这里可以添加接收到位姿后的处理逻辑
        RCLCPP_INFO(this->get_logger(), "Get a Pose");
        if (last_pose == *msg)
        {
            RCLCPP_INFO(this->get_logger(), "Pose is the same as last time, no need to plan again");
        }
        else
        {
            last_pose = pose;
            pose = *msg;
            pub_goal->publish(pose);
        }

        // 这里可以添加调用MoveGroupInterface进行规划和执行的代码
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<EegoalNode>("goal_pose_node");

    // Initialize MoveGroupInterface after the node is fully constructed

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}